
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#pragma once

#include <iostream>
#include <unistd.h>
#include <iomanip>
#include <map>
#include <utility>
#include <vector>
#include <mutex>
#include <opencv2/opencv.hpp>
#include <opencv4/opencv2/imgproc/types_c.h>
#include <librealsense2/rs.hpp>

/**
    The how_to class provides several functions for common usages of the sensor API
*/
class how_to
{
public:
    static rs2::device get_a_realsense_device(const char* my_serial)
    {
        // First, create a rs2::context.
        // The context represents the current platform with respect to connected devices
        rs2::context ctx;

        // Using the context we can get all connected devices in a device list
        rs2::device_list devices = ctx.query_devices();

        rs2::device selected_device;
        if (devices.size() == 0)
        {
            std::cerr << "No device connected, please connect a RealSense device" << std::endl;

            //To help with the boilerplate code of waiting for a device to connect
            //The SDK provides the rs2::device_hub class
            rs2::device_hub device_hub(ctx);

            //Using the device_hub we can block the program until a device connects
            selected_device = device_hub.wait_for_device();
        }
        else
        {
            for (rs2::device device : devices)
            {
                const char* serial = device.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);

                if (strcmp(serial, my_serial) == 0)
                {
                    selected_device = device;
                    return selected_device;
                }
            }
            
            throw std::out_of_range("No match device");
        }

    }

    static void print_device_information(const rs2::device& dev)
    {
        // Each device provides some information on itself
        // The different types of available information are represented using the "RS2_CAMERA_INFO_*" enum

        std::cout << "Device information: " << std::endl;
        //The following code shows how to enumerate all of the RS2_CAMERA_INFO
        //Note that all enum types in the SDK start with the value of zero and end at the "*_COUNT" value
        for (int i = 0; i < static_cast<int>(RS2_CAMERA_INFO_COUNT); i++)
        {
            rs2_camera_info info_type = static_cast<rs2_camera_info>(i);
            //SDK enum types can be streamed to get a string that represents them
            std::cout << "  " << std::left << std::setw(20) << info_type << " : ";

            //A device might not support all types of RS2_CAMERA_INFO.
            //To prevent throwing exceptions from the "get_info" method we first check if the device supports this type of info
            if (dev.supports(info_type))
                std::cout << dev.get_info(info_type) << std::endl;
            else
                std::cout << "N/A" << std::endl;
        }
    }

    static std::string get_device_name(const rs2::device& dev)
    {
        // Each device provides some information on itself, such as name:
        std::string name = "Unknown Device";
        if (dev.supports(RS2_CAMERA_INFO_NAME))
            name = dev.get_info(RS2_CAMERA_INFO_NAME);

        // and the serial number of the device:
        std::string sn = "########";
        if (dev.supports(RS2_CAMERA_INFO_SERIAL_NUMBER))
            sn = std::string("#") + dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);

        return name + " " + sn;
    }

    static bool is_right_profile(rs2::stream_profile profile, rs2_stream stream_type, rs2_format format, int width, int height, int fps)
    {
        if (profile.stream_type() == stream_type && profile.format() == format && profile.fps() == fps)
        {
            if (profile.is<rs2::video_stream_profile>())
            {
                rs2::video_stream_profile video_stream_profile = profile.as<rs2::video_stream_profile>();
                if (video_stream_profile.width() == width && video_stream_profile.height() == height)
                {
                    return true;
                }
            }
            
        }
        return false;
    }

    static void start_streaming_a_profile(const rs2::sensor& sensor, const rs2::stream_profile& stream_profile, rs2::frame& image)
    {
        sensor.open(stream_profile);

        std::mutex once;
        int status = 0;

        sensor.start([&](rs2::frame frame) { 
            once.lock();
            std::cout << "lock on" << std::endl;
            auto vf = frame.as<rs2::video_frame>();
            const int w = vf.get_width();
            const int h = vf.get_height();

            if (status == 0 && frame.get_profile().format() == RS2_FORMAT_RGB8)
            {
                std::cout << "get color image : " << "width : " << w << " hight : " << h << std::endl;
                image = frame;
                status ++;
            }
            else if (status == 0 && frame.get_profile().format() == RS2_FORMAT_Z16)
            {
                std::cout << "get depth image : " << "width : " << w << " hight : " << h  << std::endl;
                image = frame;
                status ++;
            }
            once.unlock();
            std::cout << "unlock" << std::endl;
         });

        while (status == 0)
        {
            usleep(100000);
        }

        sensor.stop();
        sensor.close();

        status = 0;
    }
};
